#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <vector>
using namespace std;

ros::Publisher   marker_pub;
ros::Publisher   cmd_vel_pub;

void shutdown(int sig);
void odomCallback(const nav_msgs::OdometryConstPtr &msg);
void init_markers(visualization_msgs::Marker *marker,geometry_msgs::Pose pose);

int main(int argc, char** argv)
{
  ros::init(argc, argv, "goal_nav");
  std::string topic = "/cmd_vel";
  ros::NodeHandle node("~"); //“~”表示相对命名，使用包名作为默认命名空间
  ros::Publisher   marker_pub = node.advertise<visualization_msgs::Marker>("waypoint_markers", 10);
  ros::Publisher   cmd_vel_pub = node.advertise<geometry_msgs::Twist>(topic, 5);

  std::vector<double> goal_vec(6,0.0);
  double wait_time = 500.0;
  node.param<std::vector<double>>("goal_vec",goal_vec,std::vector<double>(6,0)); //加上“/”后，即使前面node("~")也不会相对化
  node.param<double>("wait_time",wait_time,500.0);
  cout<<"goal vec: ";
  for(auto i: goal_vec) cout<<i<<" ";
  cout<<endl;
  cout<<"wait_time: "<<wait_time<<endl;
  visualization_msgs::Marker marker;
  geometry_msgs::Pose geo_pose;

  geo_pose.position.x =  goal_vec[3];
  geo_pose.position.y =  goal_vec[4];
  geo_pose.position.z =  goal_vec[5];
  // for navigation, roll and pitch cannot set any value!!!
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0, goal_vec[2]);
  cout<<"orientation: "<<geo_pose.orientation.x<<" "<<geo_pose.orientation.y<<" "
    <<geo_pose.orientation.z<<" "<<geo_pose.orientation.w<<endl;
  init_markers(&marker,geo_pose);

  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
  signal(SIGINT, shutdown);
  ROS_INFO("Waiting for move_base action server...");

  if(!ac.waitForServer(ros::Duration(60)))
  {
    ROS_INFO("Can't connected to move base server");
    return 1;
  }
  ROS_INFO("Connected to move base server");

  while(ros::ok())
  {
     //Update the marker display
     marker_pub.publish(marker);
     //Intialize the waypoint goal
     move_base_msgs::MoveBaseGoal goal;
     //Use the map frame to define goal poses
     goal.target_pose.header.frame_id = "map";
     //Set the time stamp to "now"
     goal.target_pose.header.stamp = ros::Time::now();
     //Set the goal pose to the i-th waypoint
     goal.target_pose.pose = geo_pose;
     
     ac.sendGoal(goal);
    //Allow 3 minute to get there
    bool finished_within_time = ac.waitForResult(ros::Duration(wait_time));
    //If we dont get there in time, abort the goal
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
    
    
  }
  ROS_INFO("move_base_square.cpp end...");
  return 0;
}


void shutdown(int sig)
{
  cmd_vel_pub.publish(geometry_msgs::Twist());
  ros::Duration(1).sleep(); // sleep for  a second
  ROS_INFO("nav_square.cpp ended!");
  ros::shutdown();
}

void init_markers(visualization_msgs::Marker *marker,geometry_msgs::Pose pose)
{
  marker->ns       = "waypoints";
  marker->id       = 0;
  marker->type     = visualization_msgs::Marker::CUBE_LIST;
  marker->action   = visualization_msgs::Marker::ADD;
  marker->lifetime = ros::Duration();//0 is forever
  marker->scale.x  = 0.2;
  marker->scale.y  = 0.2;
  marker->color.r  = 1.0;
  marker->color.g  = 0.7;
  marker->color.b  = 1.0;
  marker->color.a  = 1.0;
  marker->header.frame_id = "odom";
  marker->header.stamp = ros::Time::now();

  marker->points.push_back(pose.position);

}
